#include "app.h"

#define GET_BATT_VOL(adcmv) ((adcmv)/(1.5f/(1.5f+10.5f)))
#define GET_MOTOR_CURRENT(adcmv) ( (((adcmv)/50.0f))/5.0f )

extern LowerFilter vol_lower_filter;
extern LowerFilter current_lower_filter;

extern LowerFilter angle_lower_filter1;
extern LowerFilter angle_lower_filter2;

void update_batt_sensor(void)
{
	uint16_t vs = get_adc_value(ADC_V_CHANNEL);
	float av = adc_convert_vol(vs); //mv
	
	
	uint16_t c = get_adc_value(ADC_I_CHANNEL);
	float cv = adc_convert_vol(c); //mv
	
	lower_filter_apply(&vol_lower_filter, av);
	lower_filter_apply(&current_lower_filter, cv);
}

void update_angle_sensor(void)
{
	uint16_t agv1 = get_adc_value(ADC_ANG1_CHANNEL);
	uint16_t agv2 = get_adc_value(ADC_ANG2_CHANNEL);
	
	float a1 = adc_convert_vol(agv1);
	float a2 = adc_convert_vol(agv2);
	
	lower_filter_apply(&angle_lower_filter1, a1);
	lower_filter_apply(&angle_lower_filter2, a2);
}

float get_batt_vol(void)
{	
	return GET_BATT_VOL(vol_lower_filter.output);
}

float get_motor_current(void)
{	
	return GET_MOTOR_CURRENT(current_lower_filter.output);
}

float get_ang1_col(void)
{
	return angle_lower_filter1.output; //mv
}

float get_an2_vol(void)
{
	return angle_lower_filter2.output; //mv
}

void adc_process_test(void)
{
	char buff[32];
	
	sprintf(buff, "%.2f,%.2f,%.2f,%.2f,%.2f%\r\n", get_batt_vol()/1000.0f, get_motor_current()/1000.0f, get_ang1_col()/1000.0f, get_an2_vol()/1000.0f, get_vcc_ref()/1000.0f);
	
	debug_string(buff);
}
